
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_airspeed_ms4525.c
  * @author     baiyang
  * @date       2023-8-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_airspeed_ms4525.h"

#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
#include <analogin_manager/analogin.h>
/*-----------------------------------macro------------------------------------*/
#define MS4525D0_I2C_ADDR1 0x28
#define MS4525D0_I2C_ADDR2 0x36
#define MS4525D0_I2C_ADDR3 0x46
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool init(sensor_as_backend_t as_backend);
static bool get_differential_pressure(sensor_as_backend_t as_backend, float *pressure);
static bool get_temperature(sensor_as_backend_t as_backend, float *temperature);
static bool _probe(sensor_as_ms4525_t ms4525, uint8_t bus, uint8_t address);
static void _measure(sensor_as_ms4525_t ms4525);
static float _get_pressure(sensor_as_ms4525_const_t ms4525, int16_t dp_raw);
static float _get_temperature(int16_t dT_raw);
static void _collect(sensor_as_ms4525_t ms4525);
static void _voltage_correction(float *diff_press_pa, float *temperature);
static void _timer(void *parameter);
/*----------------------------------variable----------------------------------*/
static struct sensor_as_backend_ops ms4525_ops = {.as_backend_destructor = NULL,
                                                  .init = init,
                                                  .get_differential_pressure = get_differential_pressure,
                                                  .get_temperature = get_temperature,
                                                  .has_airspeed = NULL,
                                                  .get_airspeed = NULL,
#if MB_AIRSPEED_HYGROMETER_ENABLE
                                                  .get_hygrometer = NULL
#endif
};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_as_ms4525_ctor(sensor_as_ms4525_t ms4525, uint8_t _instance)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(ms4525, 0, sizeof(struct sensor_as_ms4525));

    sensor_as_backend_ctor(&ms4525->backend, &ms4525_ops, "ms4525", _instance);
}

sensor_as_backend_t sensor_as_ms4525_probe(uint8_t _instance)
{
    sensor_as_ms4525_t sensor = (sensor_as_ms4525_t)rt_malloc(sizeof(struct sensor_as_ms4525));

    if (!sensor) {
        return NULL;
    }

    sensor_as_ms4525_ctor(sensor, _instance);

    return (sensor_as_backend_t)sensor;
}

// probe and initialise the sensor
static bool init(sensor_as_backend_t as_backend)
{
    sensor_as_ms4525_t ms4525 = (sensor_as_ms4525_t)as_backend;

    static const uint8_t addresses[] = { MS4525D0_I2C_ADDR1, MS4525D0_I2C_ADDR2, MS4525D0_I2C_ADDR3 };
    if (sensor_as_backend_bus_is_confgured(as_backend)) {
        // the user has configured a specific bus
        for (uint8_t i = 0; i < sizeof(addresses); i++) {
            if (_probe(ms4525, sensor_as_backend_get_bus(as_backend), addresses[i])) {
                goto found_sensor;
            }
        }
    } else {
        // if bus is not configured then fall back to the old
        // behaviour of probing all buses, external first
        FOREACH_I2C_EXTERNAL(bus) {
            for (uint8_t i = 0; i < sizeof(addresses); i++) {
                if (_probe(ms4525, bus, addresses[i])) {
                    goto found_sensor;
                }
            }
        }
        FOREACH_I2C_INTERNAL(bus) {
            for (uint8_t i = 0; i < sizeof(addresses); i++) {
                if (_probe(ms4525, bus, addresses[i])) {
                    goto found_sensor;
                }
            }
        }
    }

    GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MS4525[%u]: no sensor found", sensor_as_backend_get_instance(as_backend));
    console_printf("MS4525[%u]: no sensor found", sensor_as_backend_get_instance(as_backend));
    return false;

found_sensor:
    devmgr_set_device_type(ms4525->_dev, AS_DEV_TYPE_MS4525);
    sensor_as_backend_set_bus_id(as_backend, devmgr_get_bus_id(ms4525->_dev));

    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MS4525[%u]: Found bus %u addr 0x%02x", sensor_as_backend_get_instance(as_backend), devmgr_bus_num(ms4525->_dev), devmgr_get_bus_address(ms4525->_dev));
    console_printf("MS4525[%u]: Found bus %u addr 0x%02x", sensor_as_backend_get_instance(as_backend), devmgr_bus_num(ms4525->_dev), devmgr_get_bus_address(ms4525->_dev));

    // drop to 2 retries for runtime
    devmgr_set_retries(ms4525->_dev, 2);

    devmgr_register_periodic_callback(ms4525->_dev, 20000, _timer, ms4525);
    return true;
}

// return the current differential_pressure in Pascal
static bool get_differential_pressure(sensor_as_backend_t as_backend, float *pressure)
{
    sensor_as_ms4525_t ms4525 = (sensor_as_ms4525_t)as_backend;

    rt_mutex_take(ms4525->backend._mutex, RT_WAITING_FOREVER);

    if ((time_millis() - ms4525->_last_sample_time_ms) > 100) {
        rt_mutex_release(ms4525->backend._mutex);
        return false;
    }

    if (ms4525->_press_count > 0) {
        ms4525->_pressure = ms4525->_press_sum / ms4525->_press_count;
        ms4525->_press_count = 0;
        ms4525->_press_sum = 0;
    }

    *pressure = ms4525->_pressure;

    rt_mutex_release(ms4525->backend._mutex);

    return true;
}

// return the current temperature in degrees C, if available
static bool get_temperature(sensor_as_backend_t as_backend, float *temperature)
{
    sensor_as_ms4525_t ms4525 = (sensor_as_ms4525_t)as_backend;

    rt_mutex_take(ms4525->backend._mutex, RT_WAITING_FOREVER);

    if ((time_millis() - ms4525->_last_sample_time_ms) > 100) {
        rt_mutex_release(ms4525->backend._mutex);
        return false;
    }

    if (ms4525->_temp_count > 0) {
        ms4525->_temperature = ms4525->_temp_sum / ms4525->_temp_count;
        ms4525->_temp_count = 0;
        ms4525->_temp_sum = 0;
    }

    *temperature = ms4525->_temperature;

    rt_mutex_release(ms4525->backend._mutex);

    return true;
}

// probe for a sensor
static bool _probe(sensor_as_ms4525_t ms4525, uint8_t bus, uint8_t address)
{
    ms4525->_dev = devmgr_get_i2c_device(bus, address);
    if (!ms4525->_dev) {
        return false;
    }

    // lots of retries during probe
    devmgr_set_retries(ms4525->_dev, 10);

    _measure(ms4525);
    rt_thread_mdelay(10);
    _collect(ms4525);

    return ms4525->_last_sample_time_ms != 0;
}

// start a measurement
static void _measure(sensor_as_ms4525_t ms4525)
{
    ms4525->_measurement_started_ms = 0;
    uint8_t cmd = 0;
    if (devmgr_transfer(ms4525->_dev, &cmd, 1, NULL, 0)) {
        ms4525->_measurement_started_ms = time_millis();
    }
}

/*
  this equation is an inversion of the equation in the
  pressure transfer function figure on page 4 of the datasheet
  
  We negate the result so that positive differential pressures
  are generated when the bottom port is used as the static
  port on the pitot and top port is used as the dynamic port
*/
static float _get_pressure(sensor_as_ms4525_const_t ms4525, int16_t dp_raw)
{
    const float P_max = sensor_as_backend_get_psi_range(&ms4525->backend);
    const float P_min = - P_max;
    const float PSI_to_Pa = 6894.757f;

    float diff_press_PSI  = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
    float press  = diff_press_PSI * PSI_to_Pa;
    return press;
}

/*
  convert raw temperature to temperature in degrees C
 */
static float _get_temperature(int16_t dT_raw)
{
    float temp  = ((200.0f * dT_raw) / 2047) - 50;
    return temp;
}

// read the values from the sensor
static void _collect(sensor_as_ms4525_t ms4525)
{
    uint8_t data[4];
    uint8_t data2[4];

    ms4525->_measurement_started_ms = 0;

    if (!devmgr_transfer(ms4525->_dev, NULL, 0, data, sizeof(data))) {
        return;
    }
    // reread the data, so we can attempt to detect bad inputs
    if (!devmgr_transfer(ms4525->_dev, NULL, 0, data2, sizeof(data2))) {
        return;
    }

    uint8_t status = (data[0] & 0xC0) >> 6;
    // only check the status on the first read, the second read is expected to be stale
    if (status == 2 || status == 3) {
        return;
    }

    int16_t dp_raw, dT_raw;
    dp_raw = (data[0] << 8) + data[1];
    dp_raw = 0x3FFF & dp_raw;
    dT_raw = (data[2] << 8) + data[3];
    dT_raw = (0xFFE0 & dT_raw) >> 5;

    int16_t dp_raw2, dT_raw2;
    dp_raw2 = (data2[0] << 8) + data2[1];
    dp_raw2 = 0x3FFF & dp_raw2;
    dT_raw2 = (data2[2] << 8) + data2[3];
    dT_raw2 = (0xFFE0 & dT_raw2) >> 5;

    // reject any values that are the absolute minimum or maximums these
    // can happen due to gnd lifts or communication errors on the bus
    if (dp_raw  == 0x3FFF || dp_raw  == 0 || dT_raw  == 0x7FF || dT_raw == 0 ||
        dp_raw2 == 0x3FFF || dp_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
        return;
    }

    // reject any double reads where the value has shifted in the upper more than
    // 0xFF
    if (abs(dp_raw - dp_raw2) > 0xFF || abs(dT_raw - dT_raw2) > 0xFF) {
        return;
    }

    float press  = _get_pressure(ms4525, dp_raw);
    float press2 = _get_pressure(ms4525, dp_raw2);
    float temp  = _get_temperature(dT_raw);
    float temp2 = _get_temperature(dT_raw2);
    
    if (!sensor_as_backend_disable_voltage_correction()) {
        _voltage_correction(&press, &temp);
        _voltage_correction(&press2, &temp2);
    }

    rt_mutex_take(ms4525->backend._mutex, RT_WAITING_FOREVER);

    ms4525->_press_sum += press + press2;
    ms4525->_temp_sum += temp + temp2;
    ms4525->_press_count += 2;
    ms4525->_temp_count += 2;

    ms4525->_last_sample_time_ms = time_millis();

    rt_mutex_release(ms4525->backend._mutex);
}

/**
   correct for 5V rail voltage if the system_power ORB topic is
   available

   See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
   offset versus voltage for 3 sensors
 */
static void _voltage_correction(float *diff_press_pa, float *temperature)
{
    const float slope = 65.0f;
    const float temp_slope = 0.887f;

    /*
      apply a piecewise linear correction within range given by above graph
     */
    float voltage_diff = 0;

    float board_voltage = analogin_board_voltage();
    if (board_voltage > 1.0f) {  //假设board_voltage > 1.0f时，硬件有检测板载电压功能
        voltage_diff = board_voltage - 5.0f;
    }

    voltage_diff = math_constrain_float(voltage_diff, -0.7f, 0.5f);

    (*diff_press_pa) -= voltage_diff * slope;
    (*temperature) -= voltage_diff * temp_slope;
}

// 50Hz timer
static void _timer(void *parameter)
{
    sensor_as_ms4525_t ms4525 = (sensor_as_ms4525_t)parameter;

    if (ms4525->_measurement_started_ms == 0) {
        _measure(ms4525);
        return;
    }
    if ((time_millis() - ms4525->_measurement_started_ms) > 10) {
        _collect(ms4525);
        // start a new measurement
        _measure(ms4525);
    }
}

/*------------------------------------test------------------------------------*/


